/** 
 * Columbia University COMS W4172 - 3D UI Spring 2010 Course Project
 * Ping - Augmented Reality game involving Balls, Paddles, and Sound
 * Jie Qi, Xiang Shi, Manuel Entrena, Shrenik Sadalgi
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Xml;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

using GoblinXNA;
using GoblinXNA.Graphics;
using GoblinXNA.SceneGraph;
using Model = GoblinXNA.Graphics.Model;
using GoblinXNA.Graphics.Geometry;
using GoblinXNA.Device.Capture;
using GoblinXNA.Device.Vision;
using GoblinXNA.Device.Vision.Marker;
using GoblinXNA.Device.Util;
using GoblinXNA.Device.Generic;
using GoblinXNA.Physics;
using GoblinXNA.Helpers;
using GoblinXNA.UI;
using GoblinXNA.UI.UI2D;
using GoblinXNA.Shaders;

namespace ping
{
    public partial class Ping : Microsoft.Xna.Framework.Game
    {
        GraphicsDeviceManager graphics;
        SpriteBatch spriteBatch;
        Scene scene;
        MarkerNode groundMarkerNode, toolbarMarkerNode, drumMarkerNode, dropMarkerNode;
        CameraNode cameraNodeZ, cameraNodeX, cameraNodeY;
        //GeometryNode ball;
        DirectShowCapture captureDevice;
        Boolean isPaddleSelected = false;
        Boolean isDropSelected = false;
        SpriteFont font;
        Vector3 shiftTranslation = Vector3.Zero;
        List<Paddle> paddles;
        List<Ball> balls;
        List<DropPoint> dropPoints;
        List<TransformNode> paddleTransforms;
        Paddle highlightedPaddle;
        DropPoint highlightedDrop;
        Paddle selectedPaddle;
        DropPoint selectedDrop;
        GeometryNode groundNode, lavaFieldThatDestroysBallsNode;
        GeometryNode pseudoGroundNode;
        GeometryNode axis;
        GeometryNode arrow1;
        GeometryNode arrow2;
        GeometryNode pseudoLavaNode;
        TransformNode lavaTrans;
        Boolean showHelp = true;
        Rectangle helpMainRect = new Rectangle(10, 10, 780, 580);

        TransformNode rotationTrans;
        Vector4 selectedNodeColor;
        Vector4 highlightedNodeColor;
        int idleCount = 0;
        int numBalls = 0;
        int ballDropSpeed = 0;
        int pauseDropSpeed = 0;
        int keyPressLatency;
        public int offSet = 8;
        NewtonMaterial[] physicsMaterials;
        double elapsedBase, previousElapsed;

        
        /* For map animation */
        int maxX = 1024;
        int maxY = 768;
        int mapWidth = 240;
        int mapHeight = 150;
        int mapX = 0;
        int zMapY = 0;
        int xMapY = 0;
        int yMapY = 0;
        long mouseGestureTime = 0;

        ResolveTexture2D xMapTexture;
        ResolveTexture2D yMapTexture;
        ResolveTexture2D zMapTexture;

        int animateMapXIncrement = 0;
        int animateMapYIncrement = 0;



        private TransformNode indicatorTrans, indicatorTrans2;
        GeneralShader shader;
        private Boolean modifyMode = false;
        Vector3 paddleScale, paddleTranslation;
        Quaternion paddleRotation;
        private Boolean disappeared = false;
        private Boolean highlight = false;

        int numPaddles = 0;
        int numDrops = 0;
        Boolean isGamePaused = false;
        private TransformNode tempTransNode1, tempTransNode2;
        int mapPosition;
        Boolean mapVisible;
        Boolean showPaths;
        int pathCount;
        Model pathModel;

        // UI
        private int position = -120;
        private int speed = 40;
        Rectangle doneRect = new Rectangle(-120, 60, 100, 68);
        Rectangle rotateRect = new Rectangle(-120, 138, 100, 68);
        Rectangle translateRect = new Rectangle(-120, 216, 100, 68);
        Rectangle scaleRect = new Rectangle(-120, 294, 100, 68);
        Rectangle noteRect = new Rectangle(-120, 372, 100, 68);
        Rectangle deleteRect = new Rectangle(-120, 450, 100, 68);
        
        private int menuNum = 0;
        private Boolean menuSelected = false;
        private Boolean rotateMode = false;
        private Boolean translateMode = false;
        private Boolean scaleMode = false;
        private Boolean noteMode = false;

        private Boolean systemMode = false;
        private int sysPosition = -120;

        Rectangle sysDoneRect = new Rectangle(-120, 60, 100, 68);
        Rectangle sysPathRect = new Rectangle(-120, 138, 100, 68);
        Rectangle sysSaveRect = new Rectangle(-120, 216, 100, 68);
        Rectangle sysLoadRect = new Rectangle(-120, 294, 100, 68);
        Rectangle sysHelpRect = new Rectangle(-120, 372, 100, 68);

        private int sysMenuNum = 0;
        private int sysCount = 0;
        private Boolean sysMenuSelected = false;
        private Boolean sysDoneMode = false;
        private Boolean sysPathMode = false;
        private Boolean sysSaveMode = false;
        private Boolean sysLoadMode = false;
        private Boolean sysHelpMode = false;

        private Color messageColor = Color.Cyan;

        public Ping()
        {
            graphics = new GraphicsDeviceManager(this)
                                               {
                                                    IsFullScreen = true,
                                                    PreferredBackBufferWidth = 800,
                                                    PreferredBackBufferHeight = 600
                                                };
            Content.RootDirectory = "Content";
            maxX = 800;
            maxY = 600;
        }

        protected override void Initialize()
        {
            // Initialize the GoblinXNA framework
            State.InitGoblin(graphics, Content, "");

            xMapTexture = new ResolveTexture2D(graphics.GraphicsDevice, State.Width, State.Height, 1, GraphicsDevice.PresentationParameters.BackBufferFormat);
            yMapTexture = new ResolveTexture2D(graphics.GraphicsDevice, State.Width, State.Height, 1, GraphicsDevice.PresentationParameters.BackBufferFormat);
            zMapTexture = new ResolveTexture2D(graphics.GraphicsDevice, State.Width, State.Height, 1, GraphicsDevice.PresentationParameters.BackBufferFormat);

            mapX = maxX;
            zMapY = 5;
            xMapY = maxY / 2 - mapHeight / 2;
            yMapY = maxY - mapHeight - 5;

            mapVisible = false;
            showPaths = false;
            pathModel = new Sphere(0.2f, 5, 5);
            keyPressLatency = 0;

            //State.InitGoblin(graphics, Content, "");
            // Initialize the scene graph
            scene = new Scene(this);

            scene.BackgroundColor = Color.White;
            // Initialize shader
            shader = new GeneralShader();

            // Initialize physics engine
            scene.PhysicsEngine = new NewtonPhysics();
            scene.PhysicsEngine.Gravity = 10f;
            scene.PhysicsEngine.GravityDirection = new Vector3(0, -1, 0);
            ((NewtonPhysics)scene.PhysicsEngine).MaxSimulationSubSteps = 10;
            ((NewtonPhysics)scene.PhysicsEngine).GravityDirection = new Vector3(0, 0, -1);

            // Create our video capture device that uses DirectShow library. Note that 
            // the combinations of resolution and frame rate that are allowed depend on 
            // the particular video capture device. Thus, setting incorrect resolution 
            // and frame rate values may cause exceptions or simply be ignored, depending 
 // on the device driver.  The values set here will work for a Microsoft VX 6000, 
            // and many other webcams.
            captureDevice = new DirectShowCapture();
            captureDevice.InitVideoCapture(0, FrameRate._30Hz, Resolution._640x480,
                ImageFormat.R8G8B8_24, false);

            // Add this video capture device to the scene so that it can be used for
            // the marker tracker
            scene.AddVideoCaptureDevice(captureDevice);
            CreateCameras();
            
            // Set up optical marker tracking
            // Note that we don't create our own camera when we use optical marker
            // tracking. It'll be created automatically
            SetupMarkerTracking();

            // Create the ground that represents the physical ground marker array
            CreateGround();
            //Different cameras for map view
            groundMarkerNode.AddChild(cameraNodeZ);
            groundMarkerNode.AddChild(cameraNodeX);
            groundMarkerNode.AddChild(cameraNodeY);
            // Set up the lights used in the scene
            CreateLights();

            // Create toolbar 
            CreateToolbar();
            createDropMarker();

            String a = State.Content.RootDirectory +
                    State.GetSettingVariable("AudioDirectory") + "ping.xgs";

            // Initialize sound
            Sound.Initialize("ping");


            // Create Objects
            paddles = new List<Paddle>();
            balls = new List<Ball>();
            dropPoints = new List<DropPoint>();
            paddleTransforms = new List<TransformNode>();
            createSource();
            createPaddle();
            tempTransNode1 = new TransformNode();
            tempTransNode1.Translation = new Vector3(-10, 0, 8);
            tempTransNode1.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.ToRadians(90));
            createPaddle(tempTransNode1, 0);
            tempTransNode2 = new TransformNode();
            tempTransNode2.Translation = new Vector3(-20, 2, 4);
            tempTransNode2.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.ToRadians(50));
            createPaddle(tempTransNode2, 0);

            createRotationAxisModel();

            // Set up physics material interaction
            
            NewtonMaterial physicsMaterial2 = new NewtonMaterial();
            physicsMaterial2.MaterialName1 = "ball";
            physicsMaterial2.MaterialName2 = "ball";
            physicsMaterial2.Collidable = false;

            NewtonMaterial physicsMaterial3 = new NewtonMaterial();
            physicsMaterial3.MaterialName1 = "ball";
            physicsMaterial3.MaterialName2 = "lava";

            physicsMaterial3.ContactBeginCallback = delegate(IPhysicsObject physObj1, IPhysicsObject physObj2)
            {
                Ball b = (Ball) physObj1.Container;
                deleteBall(b);
            };

            initializeMaterials();

                // Add this physics material interaction specifications to the physics engine
            ((NewtonPhysics)scene.PhysicsEngine).AddPhysicsMaterial(physicsMaterial2);
            ((NewtonPhysics)scene.PhysicsEngine).AddPhysicsMaterial(physicsMaterial3);

            // Use per pixel lighting for better quality (If you using non NVidia graphics card,
            // setting this to true may reduce the performance significantly)
            scene.PreferPerPixelLighting = false;

            // Enable shadow mapping
            // NOTE: In order to use shadow mapping, you will need to add 'PostScreenShadowBlur.fx'
            // and 'ShadowMap.fx' shader files as well as 'ShadowDistanceFadeoutMap.dds' texture file
            // to your 'Content' directory
            scene.EnableShadowMapping = true;
            State.ShowNotifications = true;
            Notifier.FadeOutTime = 2000;
            Notifier.Placement = Notifier.NotifierPlacement.TopLeft;
            Notifier.Color = Color.LawnGreen;

            KeyboardInput.Instance.KeyPressEvent += new HandleKeyPress(keyPressHandler);
            MouseInput.Instance.MouseClickEvent += new HandleMouseClick(mouseClickHandler);
            MouseInput.Instance.MouseWheelMoveEvent += new HandleMouseWheelMove(mouseWheelHandler);
            MouseInput.Instance.MouseMoveEvent += new HandleMouseMove(mouseMoveHandler);

            elapsedBase = 0;
            previousElapsed = 0;
            /* Deleting a ball when it leaves the world */
            base.Initialize();
        }

        private void createRotationAxisModel()
        {
            axis = new GeometryNode();
            axis.Model = new Cylinder(0.2f, 0.2f, 30, 3);
            axis.Model.CastShadows = true;
            axis.Material.Diffuse = Color.LawnGreen.ToVector4();
            TransformNode axisTrans = new TransformNode();
            axisTrans.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathHelper.ToRadians(90));
            axisTrans.AddChild(axis);
            arrow1 = new GeometryNode();
            arrow1.Model = new Cylinder(0.6f, 0.01f, 2, 3);
            arrow1.Model.CastShadows = true;
            arrow1.Material.Diffuse = Color.LawnGreen.ToVector4();
            TransformNode arrow1Trans = new TransformNode();
            arrow1Trans.Translation = new Vector3(15, 0, 0);
            arrow1Trans.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathHelper.ToRadians(-90));
            arrow1Trans.AddChild(arrow1);
            arrow2 = new GeometryNode();
            arrow2.Model = new Cylinder(0.6f, 0.01f, 2, 3);
            arrow2.Model.CastShadows = true;
            arrow2.Material.Diffuse = Color.LawnGreen.ToVector4();
            TransformNode arrow2Trans = new TransformNode();
            arrow2Trans.Translation = new Vector3(-15, 0, 0);
            arrow2Trans.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), MathHelper.ToRadians(90));
            arrow2Trans.AddChild(arrow2);
            rotationTrans = new TransformNode("RotationTrans");
            rotationTrans.AddChild(axisTrans);
            rotationTrans.AddChild(arrow1Trans);
            rotationTrans.AddChild(arrow2Trans);
        }


        private void CreateLights()
        {
            // Create a directional light source
            LightSource lightSource1 = new LightSource();
            lightSource1.Direction = new Vector3(0, 0, -1);
            lightSource1.Diffuse = Color.White.ToVector4();
            lightSource1.Specular = new Vector4(0.2f, 0.2f, 0.2f, 1);

            // Create a light node to hold the light source
            LightNode lightNode1 = new LightNode();
            lightNode1.LightSources.Add(lightSource1);

            groundMarkerNode.AddChild(lightNode1);

            /*// Create a spotting light source
            LightSource lightSource2 = new LightSource();
            lightSource2.Type = LightType.SpotLight;
            lightSource2.Falloff = 2f;
            lightSource2.Attenuation0 = 1.0f;
            lightSource2.Attenuation1 = 0.01f;
            lightSource2.Attenuation2 = 0.0f;
            lightSource2.Range = 5;

            lightSource2.Direction = new Vector3(0, 0, -1);
            lightSource2.InnerConeAngle = MathHelper.ToRadians(30);
            lightSource2.OuterConeAngle = MathHelper.ToRadians(35);
            lightSource2.Diffuse = Color.RoyalBlue.ToVector4();
            lightSource2.Specular = Color.White.ToVector4();

            LightNode lightNode2 = new LightNode();
            lightNode2.LightSources.Add(lightSource2);
            TransformNode lightTrans2 = new TransformNode();
            lightTrans2.Translation = new Vector3(0, 0, 50);
            lightTrans2.AddChild(lightNode2);
            groundMarkerNode.AddChild(lightTrans2);*/

            // Create another directional light source
            LightSource lightSource2 = new LightSource();
            lightSource2.Direction = new Vector3(-1, 0, 0);
            lightSource2.Diffuse = Color.White.ToVector4();
            lightSource2.Specular = new Vector4(0.2f, 0.2f, 0.2f, 1);

            // Create a light node to hold the light source
            LightNode lightNode2 = new LightNode();
            lightNode2.LightSources.Add(lightSource2);

            groundMarkerNode.AddChild(lightNode2);

            // Create another
            LightSource lightSource3 = new LightSource();
            lightSource3.Direction = new Vector3(1, 0, 0);
            lightSource3.Diffuse = Color.White.ToVector4();
            lightSource3.Specular = new Vector4(0.2f, 0.2f, 0.2f, 1);

            // Create a light node to hold the light source
            LightNode lightNode3 = new LightNode();
            lightNode3.LightSources.Add(lightSource3);

            groundMarkerNode.AddChild(lightNode3);
        }

        private void CreateCameras()
        {
            // Set up the camera of the scene graph
            Camera camera = new Camera();
            Camera camera2 = new Camera();
            Camera camera3 = new Camera();

            // Put the camera at (0, 0, 0)
            camera.Translation = new Vector3(0, 0, 100);
            // Rotate the camera -20 degrees along the X axis
            camera.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitX,
                MathHelper.ToRadians(0));
            // Set the vertical field of view to be 45 degrees
            camera.FieldOfViewY = MathHelper.ToRadians(45);
            // Set the near clipping plane to be 0.1f unit away from the camera
            camera.ZNearPlane = 0.1f;
            // Set the far clipping plane to be 1000 units away from the camera
            camera.ZFarPlane = 1000;

            // Now assign this camera to a camera node, and add this camera node to our scene graph
            cameraNodeZ = new CameraNode(camera);

            camera2.Translation = new Vector3(100, 0, 20);
            camera2.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitY,
                MathHelper.ToRadians(90)) * Quaternion.CreateFromAxisAngle(Vector3.UnitZ,
                MathHelper.ToRadians(90));
            camera2.FieldOfViewY = MathHelper.ToRadians(45);
            camera2.ZNearPlane = 0.1f;
            camera2.ZFarPlane = 1000;

            cameraNodeX = new CameraNode(camera2);

            camera3.Translation = new Vector3(0, -100, 20);
            camera3.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.ToRadians(90));
            camera3.FieldOfViewY = MathHelper.ToRadians(45);
            camera3.ZNearPlane = 0.1f;
            camera3.ZFarPlane = 1000;

            cameraNodeY = new CameraNode(camera3);
        }

        private void SetupMarkerTracking()
        {
            IMarkerTracker tracker = null;
            // Create an optical marker tracker that uses ALVAR library
            tracker = new ALVARMarkerTracker();
            ((ALVARMarkerTracker)tracker).MaxMarkerError = 0.02f;
            tracker.InitTracker(captureDevice.Width, captureDevice.Height, "calib.xml", 9.0);

            // Set the marker tracker to use for our scene
            scene.MarkerTracker = tracker;

            // Display the camera image in the background. Note that this parameter should
            // be set after adding at least one video capture device to the Scene class.
            scene.ShowCameraImage = true;
        }


        private void CreateGround()
        {
            // Create a marker node to track a ground marker array
            int[] ids = { 1, 2, 3, 4, 5, 8, 9, 10, 11, 12, 15, 16, 17, 18, 19 };
            groundMarkerNode = new MarkerNode(scene.MarkerTracker, "ALVARGroundArray.txt", ids);

            groundNode = new GeometryNode("Ground");
            
            // Add shading
            //groundNode.Model.Shader = shader;
            Material groundMaterial = new Material();
            groundMaterial.Diffuse = Color.Silver.ToVector4();
            groundMaterial.Specular = Color.White.ToVector4();
            groundMaterial.SpecularPower = 10;
            groundNode.Material = groundMaterial;


            groundNode.Model = new Box(95, 59, 0.01f);
            groundNode.Physics.MaterialName = "ground";
            groundNode.Physics.Interactable = false;
            groundNode.Physics.Collidable = true;
            groundNode.Physics.Mass = 10000;
            groundNode.Physics.Shape = GoblinXNA.Physics.ShapeType.Box;
            groundNode.AddToPhysicsEngine = true;

            // Set this ground model to act as an occluder so that it appears transparent
            groundNode.IsOccluder = true;

            // Make the ground model to receive shadow casted by other paddles with
            // CastShadows set to true
            groundNode.Model.ReceiveShadows = true;

            pseudoGroundNode = new GeometryNode("pseudoGround");
            pseudoGroundNode.Model = new Box(95, 59, 0.01f);
            
            // Add shading
            //pseudoGroundNode.Model.Shader = shader;
            Material pseudoGroundMaterial = new Material();
            pseudoGroundMaterial.Diffuse = Color.Silver.ToVector4();
            pseudoGroundMaterial.Specular = Color.White.ToVector4();
            pseudoGroundMaterial.SpecularPower = 10;
            
            pseudoGroundNode.Material = pseudoGroundMaterial;


            pseudoGroundNode.Physics.MaterialName = "pseudoGround";
            pseudoGroundNode.Physics.Interactable = false;
            pseudoGroundNode.Physics.Collidable = false;
            pseudoGroundNode.Physics.Mass = 10000;
            pseudoGroundNode.Physics.Shape = GoblinXNA.Physics.ShapeType.Box;
            pseudoGroundNode.AddToPhysicsEngine = false;

            // Make the pseudoGround model to receive shadow casted by other paddles with
            // CastShadows set to true
            pseudoGroundNode.Model.ReceiveShadows = true;

            lavaFieldThatDestroysBallsNode = new GeometryNode("lavaFieldThatDestroysBallsNode");
            lavaFieldThatDestroysBallsNode.Model = new Box(500, 500, 1);

            lavaFieldThatDestroysBallsNode.IsOccluder = true;
            lavaFieldThatDestroysBallsNode.Physics.MaterialName = "lava";
            lavaFieldThatDestroysBallsNode.Physics.Interactable = true;
            lavaFieldThatDestroysBallsNode.Physics.Collidable = true;

            pseudoLavaNode = new GeometryNode("pseudoLava");
            pseudoLavaNode.Model = new Box(500, 500, 1);
            Material pseudoLavaMaterial = new Material();
            pseudoLavaMaterial.Diffuse = Color.White.ToVector4();
            pseudoLavaMaterial.Specular = Color.White.ToVector4();
            pseudoLavaMaterial.SpecularPower = 10;
            pseudoLavaNode.Material = pseudoLavaMaterial;
            pseudoLavaNode.Physics.MaterialName = "pseudoLava";
            pseudoLavaNode.Physics.Interactable = false;
            pseudoLavaNode.Physics.Collidable = false;
            pseudoLavaNode.Physics.Mass = 10000;
            pseudoLavaNode.Physics.Shape = GoblinXNA.Physics.ShapeType.Box;
            pseudoLavaNode.AddToPhysicsEngine = false;
            pseudoLavaNode.Model.ReceiveShadows = true;

            lavaTrans = new TransformNode();
            lavaTrans.Translation = new Vector3(0, 0, -100);
            lavaTrans.AddChild(lavaFieldThatDestroysBallsNode);

            groundMarkerNode.AddChild(groundNode);
            groundMarkerNode.AddChild(lavaTrans);

            scene.RootNode.AddChild(groundMarkerNode);
        }

        private void CreateToolbar()
        {
            // Create a marker node to track a toolbar marker array.
            int[] ids1 = new int[1];
            ids1[0] = 29;
            toolbarMarkerNode = new MarkerNode(scene.MarkerTracker, "Toolbar.txt", ids1);

            GeometryNode toolbarNode = new GeometryNode("Toolbar");
            toolbarNode.Model = new Box(10, 10, 0.1f);
            

            // Set this ground model to act as an occluder so that it appears transparent
            toolbarNode.IsOccluder = true;

            // Add a ball on the tool bar to indicate how the distance is computed
            GeometryNode indicator = new GeometryNode("indicator");
            indicator.Model = new Sphere(1f, 5, 5);
            
            Material indicatorMaterial = new Material();
            indicatorMaterial.Diffuse = Color.Blue.ToVector4();
            indicatorMaterial.Specular = Color.White.ToVector4();
            indicatorMaterial.SpecularPower = 10;
            indicator.Material = indicatorMaterial;

            indicatorTrans = new TransformNode();
            indicatorTrans.Translation = new Vector3(0, 0, -0.1f);
            indicatorTrans.AddChild(indicator);
            toolbarMarkerNode.AddChild(indicatorTrans);
      
            toolbarMarkerNode.AddChild(toolbarNode);
            scene.RootNode.AddChild(toolbarMarkerNode);
        }

        /*private void createDrum()
        {
            // Create a marker node to track a toolbar marker array.
            int[] ids1 = new int[1];
            ids1[0] = 30;
            drumMarkerNode = new MarkerNode(scene.MarkerTracker, "Toolbar2.txt", ids1);

            GeometryNode drumNode = new GeometryNode("Toolbar");
            drumNode.Model = new Box(10, 10, 0.1f);


            // Set this ground model to act as an occluder so that it appears transparent
            drumNode.IsOccluder = true;

            // Add a ball on the tool bar to indicate how the distance is computed
            GeometryNode indicator = new GeometryNode("indicator");
            indicator.Model = new Sphere(1f, 5, 5);

            Material indicatorMaterial = new Material();
            indicatorMaterial.Diffuse = Color.Blue.ToVector4();
            indicatorMaterial.Specular = Color.White.ToVector4();
            indicatorMaterial.SpecularPower = 10;
            indicator.Material = indicatorMaterial;

            indicatorTrans = new TransformNode();
            indicatorTrans.Translation = new Vector3(0, 0, 0);
            indicatorTrans.AddChild(indicator);
            drumMarkerNode.AddChild(indicatorTrans);

            drumMarkerNode.AddChild(drumNode);
            scene.RootNode.AddChild(drumMarkerNode);
        }*/

        private void createDropMarker()
        {
            // Create a marker node to track a toolbar marker array.
            int[] ids1 = new int[1];
            ids1[0] = 30;
            dropMarkerNode = new MarkerNode(scene.MarkerTracker, "Toolbar2.txt", ids1);

            GeometryNode dropNode = new GeometryNode("dropBar");
            dropNode.Model = new Box(10, 10, 0.1f);


            // Set this ground model to act as an occluder so that it appears transparent
            dropNode.IsOccluder = true;

            // Add a ball on the tool bar to indicate how the distance is computed
            GeometryNode indicator = new GeometryNode("indicator");
            indicator.Model = new Sphere(1f, 5, 5);

            Material indicatorMaterial = new Material();
            indicatorMaterial.Diffuse = Color.Blue.ToVector4();
            indicatorMaterial.Specular = Color.White.ToVector4();
            indicatorMaterial.SpecularPower = 10;
            indicator.Material = indicatorMaterial;

            indicatorTrans = new TransformNode();
            indicatorTrans.Translation = new Vector3(0, 0, 0);
            indicatorTrans.AddChild(indicator);
            dropMarkerNode.AddChild(indicatorTrans);

            dropMarkerNode.AddChild(dropNode);
            scene.RootNode.AddChild(dropMarkerNode);
        }

        private void createSource()
        {
            DropPoint drop = new DropPoint();
            drop.Name = "Drop_" + numDrops++;
            groundMarkerNode.AddChild(drop.Trans);
            dropPoints.Add(drop);
        }

        private void createSource(Vector3 position)
        {
            DropPoint drop = new DropPoint(position);
            drop.Name = "Drop_" + numDrops++;
            groundMarkerNode.AddChild(drop.Trans);
            dropPoints.Add(drop);
        }

        private void deleteBall(Ball b)
        {
            TransformNode ballTransform = (TransformNode)b.Parent;
            MarkerNode marker = (MarkerNode)ballTransform.Parent;
            marker.RemoveChild(ballTransform);
            ballTransform.RemoveChild(b);
            ((NewtonPhysics) scene.PhysicsEngine).DisableSimulation(b.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(b.Physics);
        }

        private void createPaddle()
        {
            TransformNode paddleTransform = new TransformNode();
            paddleTransform.Translation = new Vector3(0, 0, 8);
            paddleTransform.Rotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.ToRadians(-45));
            createPaddle(paddleTransform, 0);
        }

        private void createPaddle(TransformNode paddleTransNode, int note){
            // The Paddle
            Paddle p = new Paddle(paddleTransNode);
            p.Name = "Paddle_" + numPaddles;
            p.Note = note;
            groundMarkerNode.AddChild(p.Trans);
            paddles.Add(p);
            paddleTransforms.Add(p.Trans);
            numPaddles++;
            Notifier.AddMessage("Paddle created!");
        }

        private void deletePaddle()
        {

            if (selectedPaddle == null)
                return;
            selectedPaddle.AddToPhysicsEngine = false;
            Paddle pad = selectedPaddle;
            paddles.Remove(selectedPaddle);
            paddleTransforms.Remove((TransformNode)selectedPaddle.Parent);
            unselectObject();
            groundMarkerNode.RemoveChild(pad.Parent);
            ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(pad.Physics);
            Notifier.AddMessage("Paddle deleted!");
        }


        private void deleteDropPoint()
        {

            if (highlightedDrop == null)
                return;
            highlightedDrop.AddToPhysicsEngine = false;
            DropPoint d = highlightedDrop;
            dropPoints.Remove(highlightedDrop);
            groundMarkerNode.RemoveChild(d.Trans);
            ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(d.Physics);
            highlightedDrop = null;
        }


        private void createModifyMenu(Boolean movein, int num, Boolean selected)
        {
            if (movein)
            {
                if (position + speed <= 40)
                    position += speed;
            }
            else
            {
                if (position - speed >= -120)
                    position -= speed;
            }
            doneRect.X = rotateRect.X = translateRect.X = scaleRect.X = noteRect.X = deleteRect.X = position;
            Color doneColor, rotateColor, translateColor, scaleColor, noteColor, deleteColor;
            doneColor = rotateColor = translateColor = scaleColor = noteColor = deleteColor = Color.Gray;
            Color selectedColor = Color.Orange;

            switch (num)
            {
                case 0:
                    if (selected)
                        doneColor = selectedColor;
                    else
                    {
                        doneColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to exit fine adjustment mode");
                    }
                    break;

                case 1:
                    if (selected)
                        rotateColor = selectedColor;
                    else
                    {
                        rotateColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to rotate paddle");
                    }
                    break;

                case 2:
                    if (selected)
                        translateColor = selectedColor;
                    else
                    {
                        translateColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to translate paddle");
                    }
                    break;

                case 3:
                    if (selected)
                        scaleColor = selectedColor;
                    else
                    {
                        scaleColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to scale paddle");
                    }
                    break;

                case 4:
                    if (selected)
                        noteColor = selectedColor;
                    else
                    {
                        noteColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to change musical note of paddle");
                    }

                    break;

                case 5:
                    if (selected)
                        deleteColor = selectedColor;
                    else
                    {
                        deleteColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to delete paddle");
                    }
                    break;

                default:
                    break;
            }

            UI2DRenderer.FillRectangle(doneRect, Content.Load<Texture2D>("DoneRec"), doneColor);
            UI2DRenderer.FillRectangle(rotateRect, Content.Load<Texture2D>("RotateRec"), rotateColor);
            UI2DRenderer.FillRectangle(translateRect, Content.Load<Texture2D>("TranslateRec"), translateColor);
            UI2DRenderer.FillRectangle(scaleRect, Content.Load<Texture2D>("ScaleRec"), scaleColor);
            UI2DRenderer.FillRectangle(noteRect, Content.Load<Texture2D>("NoteRec"), noteColor);
            UI2DRenderer.FillRectangle(deleteRect, Content.Load<Texture2D>("DeleteRec"), deleteColor);
            
            /*UI2DRenderer.WriteText(donePos,doneLabel, Color.Black, font);
            UI2DRenderer.WriteText(rotate1Pos, rotate1Label, Color.Black, font);
            UI2DRenderer.WriteText(rotate2Pos, rotate2Label, Color.Black, font);
            UI2DRenderer.WriteText(scalePos, scaleLabel, Color.Black, font);
            UI2DRenderer.WriteText(notePos, noteLabel, Color.Black, font);
            UI2DRenderer.WriteText(deletePos, deleteLabel, Color.Black, font);*/
        }

        private void createSystemMenu(Boolean movein, int num, Boolean selected, int count)
        {
            if (movein)
            {
                Console.WriteLine("Pos:" + (sysPosition + speed));
                if (sysPosition + speed <= 40)
                    sysPosition += speed;
            }
            else
            {
                Console.WriteLine("Pos:" + (sysPosition - speed));
                if (sysPosition - speed >= -120)
                    sysPosition -= speed;
            }
            Color sysDoneColor, sysPathColor, sysSaveColor, sysLoadColor, sysHelpColor;
            sysDoneColor = sysPathColor = sysSaveColor = sysLoadColor = sysHelpColor = Color.Gray;
            Color selectedColor = Color.Orange;
            Texture2D tx;
            sysDoneRect.X = sysPathRect.X = sysSaveRect.X = sysLoadRect.X = sysHelpRect.X = sysPosition;

            switch (num)
            {
                case 0:
                    if (count < 0)
                    {
                        sysDoneColor = selectedColor;
                    }
                    else
                    {
                        sysDoneColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to exit this menu");
                    }
                        
                    break;

                case 1:
                    if (selected)
                    {
                        sysPathColor = Color.White;
                        writeStatusMessageBottom("LEFT CLICK to hide ball paths");
                    }
                    else
                    {
                        sysPathColor = Color.White;
                        writeStatusMessageBottom("LEFT CLICK to show ball paths");
                    }
                    break;

                case 2:
                    if (count < 0)
                        sysSaveColor = selectedColor;
                    else
                    {
                        sysSaveColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to save current paddle arrangement");
                    }
                    break;

                case 3:
                    if (count < 0)
                        sysLoadColor = selectedColor;
                    else
                    {
                        sysLoadColor = Color.White;
                        if (movein) writeStatusMessageBottom("LEFT CLICK to load previously saved paddle arrangement");
                    }
                    break;

                case 4:
                    if (count < 0)
                        sysHelpColor = selectedColor;
                    else
                        sysHelpColor = Color.White;
                    break;

                default:
                    break;
            }

            if (sysPathMode)
                tx = Content.Load<Texture2D>("PathsRecON");
            else 
                tx = Content.Load<Texture2D>("PathsRecOff");

            UI2DRenderer.FillRectangle(sysDoneRect, Content.Load<Texture2D>("DoneSysRec"), sysDoneColor);
            UI2DRenderer.FillRectangle(sysPathRect, tx, sysPathColor);
            UI2DRenderer.FillRectangle(sysSaveRect, Content.Load<Texture2D>("SaveRec"), sysSaveColor);
            UI2DRenderer.FillRectangle(sysLoadRect, Content.Load<Texture2D>("LoadRec"), sysLoadColor);
            UI2DRenderer.FillRectangle(sysHelpRect, Content.Load<Texture2D>("HelpRec"), sysHelpColor);
            /*sysDonePos.X = sysPathPos.X = sysSavePos.X = sysLoadPos.X = sysPosition;
            UI2DRenderer.WriteText(sysDonePos,"Done", Color.Black, font);
            UI2DRenderer.WriteText(sysPathPos, "Paths", Color.Black, font);
            UI2DRenderer.WriteText(sysSavePos, "Save", Color.Black, font);
            UI2DRenderer.WriteText(sysLoadPos, "Load", Color.Black, font);*/
        }

        /**
         * Method to play a sound when the ball and paddle collide based on contactSpeed
         */
        private void ballPaddleCollision(float speed, Vector3 position)
        {
            int temp = (int)Math.Floor((decimal)(speed))- offSet;
            if (temp > 26) temp = 26;
            Matrix m = Matrix.CreateTranslation(position) * groundMarkerNode.WorldTransformation;
            Sound.Play3D("gpiano_" + (temp <= 1 ? 1 : temp), m.Translation);
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            // Create a new SpriteBatch, which can be used to draw textures.
            spriteBatch = new SpriteBatch(GraphicsDevice);
            font = Content.Load<SpriteFont>("DebugFont");
            // TODO: use this.Content to load your game content here
        }

        /// <summary>
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            // TODO: Unload any non ContentManager content here
            Content.Unload();
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            // Allows the game to exit
            if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
                this.Exit();

            // TODO: Add your update logic here


            if (ballDropSpeed > 0 && (((gameTime.TotalGameTime.TotalSeconds) % (10 - ballDropSpeed)) <= 0.01)) {
                foreach (DropPoint d in dropPoints)
                {
                    if (d.Enabled)
                    {
                        Ball b = d.createBall();
                        balls.Add(b);
                        groundMarkerNode.AddChild(b.Trans);
                        balls.Add(b);
                    }
                }
            }
            if(keyPressLatency > 0)
                keyPressLatency--;
            base.Update(gameTime);
            mouseGestureTime++;
        }

        /// <summary>
        /// Calculate distance between toolbar and paddles.
        /// Update highlightednode if toolbar is close to a paddle
        /// </summary>
        protected void checkDistances()
        {
            Matrix[] m = new Matrix[paddles.Count];
            highlight = false;

            /* Iterate through paddles and check the distances between them and the Toolbar */
            for (int i = 0; i < paddles.Count; i++)
            {
                Paddle o = paddles.ElementAt(i);
                TransformNode t = (TransformNode)o.Parent;
                MarkerNode marker = (MarkerNode)t.Parent;
                m[i] = t.WorldTransformation * marker.WorldTransformation;
                //m[i] = o.WorldTransformation;
                float objectDistance = Vector3.Distance(m[i].Translation, toolbarMarkerNode.WorldTransformation.Translation);

                if (objectDistance < 10 && !highlight)
                {
                    if (highlightedPaddle != null)
                    {
                        if (!highlightedPaddle.Equals(o))
                        {
                            highlightedPaddle.Material.Diffuse = highlightedNodeColor;
                        }
                    }

                    if (highlightedPaddle == null || !highlightedPaddle.Equals(o))
                    {
                        highlightedNodeColor = o.Material.Diffuse;
                        idleCount = 0;
                    }
                    else
                    {
                        idleCount++;
                        if (idleCount > 55)
                        {
                            //selectObject();
                            idleCount = 0;
                            return;
                        }
                    }
                    highlightedPaddle = o;
                    o.Material.Diffuse = Color.CornflowerBlue.ToVector4();
                    highlight = true;
                    String label1 = highlightedPaddle.Name;

                    writeStatusMessageTop(label1);
                    writeStatusMessageBottom("LEFT CLICK to free-transform, RIGHT CLICK for fine adjustment");
                }
            }
            if (!highlight)
            {

                if (highlightedPaddle != null)
                {
                    highlightedPaddle.Material.Diffuse = highlightedNodeColor;
                    highlightedPaddle = null;
                }
            }
        }

        protected void checkDistancesDrops()
        {
            Matrix[] m = new Matrix[dropPoints.Count];
            highlight = false;

            /* Iterate through paddles and check the distances between them and the Toolbar */
            for (int i = 0; i < dropPoints.Count; i++)
            {
                DropPoint o = dropPoints.ElementAt(i);
                TransformNode t = (TransformNode)o.Parent;
                MarkerNode marker = (MarkerNode)t.Parent;
                m[i] = t.WorldTransformation * marker.WorldTransformation;
                //m[i] = o.WorldTransformation;
                float objectDistance = Vector3.Distance(m[i].Translation, dropMarkerNode.WorldTransformation.Translation);

                if (objectDistance < 10 && !highlight)
                {
                    if (highlightedDrop != null)
                    {
                        if (!highlightedDrop.Equals(o))
                        {
                            highlightedDrop.Material = DropPoint.sourceMaterial;
                        }
                    }

                    if (highlightedDrop == null || !highlightedDrop.Equals(o))
                    {
                        idleCount = 0;
                    }
                    else
                    {
                        idleCount++;
                        if (idleCount > 55)
                        {
                            //selectObject();
                            idleCount = 0;
                            return;
                        }
                    }
                    highlightedDrop = o;
                    Material mate = new Material();
                    mate.Diffuse = Color.White.ToVector4();
                    o.Material = mate;
                    highlight = true;
                    String label1 = highlightedDrop.Name;
                    writeStatusMessageTop(label1);
                }
                else
                {
                    dropPoints[i].Material = DropPoint.sourceMaterial;
                }
            }
            if (!highlight)
            {

                if (highlightedDrop != null)
                {
                    highlightedDrop.Material = DropPoint.sourceMaterial;
                    highlightedDrop = null;

                }
            }
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            //If we need to display the minimap, take a screenshot from the top
            if (mapVisible)
            {
                scene.ShowCameraImage = false;

                groundMarkerNode.AddChild(pseudoGroundNode);
                lavaTrans.AddChild(pseudoLavaNode);

                scene.CameraNode = cameraNodeZ;
                base.Draw(gameTime); //Render everything
                graphics.GraphicsDevice.ResolveBackBuffer(zMapTexture); //And copy to map texture

                scene.CameraNode = cameraNodeX;
                base.Draw(gameTime); //Render everything
                graphics.GraphicsDevice.ResolveBackBuffer(xMapTexture); //And copy to map texture
                
                scene.CameraNode = cameraNodeY;
                base.Draw(gameTime); //Render everything
                graphics.GraphicsDevice.ResolveBackBuffer(yMapTexture); //And copy to map texture

                groundMarkerNode.RemoveChild(pseudoGroundNode);
                lavaTrans.RemoveChild(pseudoLavaNode);
            }

            if (showPaths)
            {
                if (pathCount == 3) //We only take snapshots for paths every 5 frames
                {
                    pathCount = 0;
                    foreach (GeometryNode n in balls) //For each ball create a path dot and add it to the scene
                    {
                            TransformNode transNode = (TransformNode)n.Parent;

                            GeometryNode newBall = new GeometryNode();
                            newBall.Model = pathModel;

                            TransformNode newTransNode = new TransformNode();
                            newTransNode.WorldTransformation = n.WorldTransformation * Matrix.Invert(transNode.WorldTransformation);
                            newTransNode.AddChild(newBall);

                            transNode.AddChild(newTransNode);
                            if (transNode.Children.Count > 20) //Don't keep more than 20 path dots per ball, it would get confusing
                                transNode.Children.RemoveAt(1);
                    }
                }
                pathCount++;
            }

            if (groundMarkerNode.MarkerFound)
            {
                if (toolbarMarkerNode.MarkerFound && !isPaddleSelected)   // find the paddle to highlight
                {
                    checkDistances();
                }
                else if (dropMarkerNode.MarkerFound && selectedDrop == null)
                {
                    checkDistancesDrops();
                }
                else if (toolbarMarkerNode.MarkerFound && isPaddleSelected && !modifyMode)    // creating new paddle
                {
                    writeStatusMessageTop(selectedPaddle.Name + " is selected"); 
                    writeStatusMessageBottom("SCROLL to change size, LEFT CLICK for fine adjustment, RIGHT CLICK to delete");
                    Matrix scaleFactor = Matrix.CreateScale(selectedPaddle.Trans.Scale);
                    Matrix mat = scaleFactor * toolbarMarkerNode.WorldTransformation *
                        Matrix.Invert(groundMarkerNode.WorldTransformation);
                    ((NewtonPhysics)scene.PhysicsEngine).SetTransform(selectedPaddle.Physics, mat);

                    Vector3 sca, trans;
                    Quaternion rot;
                    mat.Decompose(out sca, out rot, out trans);
                    selectedPaddle.Trans.Translation = trans;
                    selectedPaddle.Trans.Rotation = rot;
                    selectedPaddle.Trans.Scale = sca;
                }
                else if (!toolbarMarkerNode.MarkerFound && isPaddleSelected && !modifyMode)   // lose tracking, move the paddle out of screen
                {
                    selectedPaddle.Trans.Translation = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
                }
                else 
                {
                    if (rotateMode) //Display rotation axis
                    {
                        rotationTrans.Translation = selectedPaddle.Trans.Translation;
                        Vector3 sca, trans;
                        Quaternion rot;
                        groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
                        rot.Conjugate();
                        rotationTrans.Rotation = rot;
                        writeStatusMessageBottom("LOOK around to change axis, SCROLL to rotate paddle");
                    }
                    else if (translateMode) //Display translation axis (which is in fact the rotation axis :-) )
                    {
                        Vector3 sca, trans;
                        Quaternion rot, rotGround;
                        groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);

                        rot.Conjugate();
                        Vector3 groundNormal = new Vector3(0, 0, 1);
                        Vector3 WorldGroundNormal = Vector3.Transform(groundNormal, rot);
                        Vector3 viewRay = new Vector3(0, 0, -1);
                        Vector3 product = WorldGroundNormal * viewRay;

                        double angle = Math.Asin(product.Length() / (WorldGroundNormal.Length() * viewRay.Length()));
                        Vector3 v = new Vector3(0, 1, 0);

                        Vector3 translation = Vector3.Transform(v, rot);
                        if (MathHelper.ToDegrees((float)angle) > 45)
                        {
                            rotationTrans.Translation = selectedPaddle.Trans.Translation;
                            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
                            rotGround = rot;
                            rot.Conjugate();
                            rot.X = 0;
                            rot.Y = 0;
                            rot = Quaternion.CreateFromYawPitchRoll(0, 0, MathHelper.ToRadians(90)) * rot;

                            rotationTrans.Rotation = rot;
                        }
                        else
                        {
                            translation.X = translation.Y = 0;
                            rotationTrans.Rotation = Quaternion.CreateFromYawPitchRoll(MathHelper.ToRadians(90), 0, 0);
                        }
                        rotationTrans.Translation = selectedPaddle.Trans.Translation;
                        writeStatusMessageBottom("LOOK around to change axis, SCROLL to translate paddle");
                    }
                    else if (scaleMode)
                    {
                        writeStatusMessageBottom("SCROLL to scale paddle");
                    } else if (noteMode)
                    {
                        writeStatusMessageBottom("SCROLL to change musical note of paddle");
                    }

                    if (highlightedPaddle != null)
                    {
                        highlightedPaddle.Material.Diffuse = highlightedNodeColor;
                        highlightedPaddle = null;
                    }
                }
                if (selectedPaddle == null && highlightedPaddle == null && !modifyMode && !systemMode && !showHelp && !isGamePaused) writeStatusMessageBottom("SCROLL UP to start dropping balls, LEFT CLICK to create paddle, RIGHT CLICK to see menu");
            }
            createModifyMenu(modifyMode, menuNum, menuSelected);
            createSystemMenu(systemMode, sysMenuNum, sysMenuSelected, sysCount);

            if (isGamePaused)
            {
                writeStatusMessageBottom("Game is paused. MIDDLE CLICK to resume");
            }
            //If we drew the map we need to set the right camera back
            if (mapVisible)
            {
                scene.ShowCameraImage = true;
                scene.CameraNode = null;
                scene.MarkerTracker = scene.MarkerTracker; //This is stupid, I know, but apparently the world disappears if we don't do it :)
            }

            base.Draw(gameTime);
            
            drawMap();

            if (showHelp)
            {
                UI2DRenderer.FillRectangle(helpMainRect, Content.Load<Texture2D>("HelpMain"), Color.White);
                mapVisible = false;
            }

            
        }

        private void scalePaddleUpModify()
        {
            selectedPaddle.scaleUp();
        }

        private void scalePaddleDownModify()
        {
            selectedPaddle.scaleDown();
        }

        /*private void rotatePaddleYawUp()
        {
            Quaternion rotation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.1f);
            Vector3 sca, trans;
            Quaternion rot, rot2;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot2, out trans);
            //rotation = rotation * rot;
            rot2.Conjugate();
            ((Paddle)selectedNode).Trans.Rotation *= rot2 * rotation * rot;
            //((Paddle)selectedNode).Trans.Rotation *= Quaternion.CreateFromYawPitchRoll(0.1f, 0, 0);
        }

        private void rotatePaddleYawDown()
        {
            Quaternion rotation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), -0.1f);
            Vector3 sca, trans;
            Quaternion rot, rot2;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot2, out trans);
            //rotation = rotation * rot;
            rot2.Conjugate();
            ((Paddle)selectedNode).Trans.Rotation *= rot2 * rotation * rot;
            //((Paddle)selectedNode).Trans.Rotation *= Quaternion.CreateFromYawPitchRoll(-0.1f, 0, 0);
        }*/

        private void rotatePaddleUp()
        {
            

            Quaternion rotation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), -0.1f);
            Vector3 sca, trans;
            Quaternion rot, rot2;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot2, out trans);

            Vector3 v = new Vector3(1, 0, 0);
            //rotation = rotation * rot;
            rot2.Conjugate();
            Vector3 axis = Vector3.Transform(v, rot2);
            //((Paddle)selectedNode).Trans.Rotation *= rot2*rotation*rot;
            selectedPaddle.Trans.Rotation = Quaternion.CreateFromAxisAngle(axis, -0.1f) * selectedPaddle.Trans.Rotation;
        }

        private void rotatePaddleDown()
        {
            Vector3 v = new Vector3(1, 0, 0);

            Quaternion rotation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.1f);
            Vector3 sca, trans;
            Quaternion rot, rot2;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot2, out trans);


            //rotation = rotation * rot;
            rot2.Conjugate();
            Vector3 axis = Vector3.Transform(v, rot2);
            //((Paddle)selectedNode).Trans.Rotation *= rot2*rotation*rot;
            selectedPaddle.Trans.Rotation = Quaternion.CreateFromAxisAngle(axis, 0.1f) * selectedPaddle.Trans.Rotation;
        }

        private void movePaddleFarther()
        {
            Vector3 sca, trans;
            Quaternion rot;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);

            rot.Conjugate();
            Vector3 groundNormal = new Vector3(0,0,1);
            Vector3 WorldGroundNormal = Vector3.Transform(groundNormal, rot);
            Vector3 viewRay = new Vector3(0,0,-1);
            Vector3 product = WorldGroundNormal * viewRay;

            double angle = Math.Asin(product.Length()/(WorldGroundNormal.Length() * viewRay.Length()));
            Vector3 v = new Vector3(0, 1, 0);

            Vector3 translation = Vector3.Transform(v, rot);
            if (MathHelper.ToDegrees((float)angle) > 45)
                translation.Z = 0;
            else
            {
                translation.X = translation.Y = 0;
            }
            selectedPaddle.Trans.Translation += translation;
        }

        private void movePaddleCloser()
        {
            Vector3 sca, trans;
            Quaternion rot;
            groundMarkerNode.WorldTransformation.Decompose(out sca, out rot, out trans);

            rot.Conjugate();
            Vector3 groundNormal = new Vector3(0, 0, 1);
            Vector3 WorldGroundNormal = Vector3.Transform(groundNormal, rot);
            Vector3 viewRay = new Vector3(0, 0, -1);
            Vector3 product = WorldGroundNormal * viewRay;

            double angle = Math.Asin(product.Length() / (WorldGroundNormal.Length() * viewRay.Length()));
            Vector3 v = new Vector3(0, -1, 0);

            Vector3 translation = Vector3.Transform(v, rot);
            if (MathHelper.ToDegrees((float)angle) > 45)
                translation.Z = 0;
            else
            {
                translation.X = translation.Y = 0;
            }
            selectedPaddle.Trans.Translation += translation;
        }

        /**
         * Key handler for object selection
         */
        public void keyPressHandler(Keys key, KeyModifier k)
        {
            if (keyPressLatency == 0)
            {
                if (key == Keys.Space)
                {
                    if (!isPaddleSelected)
                        selectObject();
                    else
                        unselectObject();
                }

                if (key == Keys.Escape)
                {
                    this.Exit();
                }

                if (key == Keys.P)
                {
                    if (selectedPaddle == null)
                    {
                        createPaddle();
                        highlightedPaddle = paddles[paddles.Count - 1];
                        selectObject();
                    }
                }
                if (key == Keys.B)
                {
                    foreach (DropPoint d in dropPoints)
                    {
                        Ball b = d.createBall();
                        balls.Add(b);
                        groundMarkerNode.AddChild(b.Trans);
                        balls.Add(b);
                    }
                }
                if (key == Keys.D)
                {
                    deletePaddle();
                }
                if (key == Keys.S)
                {
                    isGamePaused = !isGamePaused;
                    if (isGamePaused)
                    {
                        for (int i = 0; i < balls.Count; i++)
                        {
                            ((NewtonPhysics)scene.PhysicsEngine).DisableSimulation(balls[i].Physics);
                        }
                        pauseDropSpeed = ballDropSpeed;
                        ballDropSpeed = 0;
                    }
                    else
                    {
                        for (int i = 0; i < balls.Count; i++)
                        {
                            ((NewtonPhysics)scene.PhysicsEngine).EnableSimulation(balls[i].Physics);
                        }
                        ballDropSpeed = pauseDropSpeed;
                    }
                }
                if (key == Keys.O)
                {
                    for (int i = 0; i < paddles.Count; i++)
                    {
                        TransformNode t = (TransformNode)(paddles.ElementAt(i).Parent);
                    }
                }

                if (key == Keys.M)
                {
                    if (!modifyMode)
                    {
                        selectObject();
                        modifyMode = true;
                    }
                    else
                    {
                        unselectObject();
                        modifyMode = false;
                    }
                }

                if (key == Keys.L)
                {
                     mapVisible = !mapVisible;
                }
                if (key == Keys.T)
                {
                    pathCount = 0;
                    showPaths = !showPaths;
                    if (showPaths == false)
                    {
                        foreach (Node n in balls)
                        {
                            TransformNode transNode = (TransformNode)n.Parent;
                            if (transNode.Children.Count > 1)
                                transNode.Children.RemoveRange(1, transNode.Children.Count - 1);
                        }
                    }
                    else
                    {
                        Notifier.AddMessage("Showing ball paths");
                    }
                }
                if (key == Keys.G)
                {
                    saveConfiguration();
                }
                if (key == Keys.H)
                {
                    loadConfiguration();
                }
                keyPressLatency = 5;
            }
        }

        private void mouseClickHandler(int button, Point p) {
            Sound.Play("button_3");
            if (showHelp)
            {
                showHelp = false;
            }
            else
            {
                switch (button)
                {
                        //Left Click
                    case 0:
                        //If we are not selecting, we create a new paddle
                        if (!systemMode && selectedPaddle == null && highlightedPaddle == null &&
                            toolbarMarkerNode.MarkerFound)
                        {
                            Matrix mat = toolbarMarkerNode.WorldTransformation*
                                         Matrix.Invert(groundMarkerNode.WorldTransformation);
                            TransformNode t = new TransformNode();
                            Vector3 sca, trans;
                            Quaternion rot;
                            mat.Decompose(out sca, out rot, out trans);
                            t.Translation = trans;
                            t.Rotation = rot;
                            t.Scale = sca;
                            createPaddle(t, 0);
                            highlightedPaddle = paddles[paddles.Count - 1];
                            highlightedNodeColor = paddles[paddles.Count - 1].Material.Diffuse;
                            selectObject();
                            modifyMode = false;
                        }
                        else if (!systemMode && selectedPaddle == null && highlightedPaddle == null &&
                                 highlightedDrop == null && dropMarkerNode.MarkerFound)
                        {
                            Matrix mat = dropMarkerNode.WorldTransformation*
                                         Matrix.Invert(groundMarkerNode.WorldTransformation);
                            TransformNode t = new TransformNode();
                            Vector3 sca, trans;
                            Quaternion rot;
                            mat.Decompose(out sca, out rot, out trans);
                            t.Translation = trans;
                            t.Rotation = rot;
                            t.Scale = sca;
                            createSource(t.Translation);
                            highlightedDrop = dropPoints[dropPoints.Count - 1];
                        }
                        else if (!systemMode && selectedPaddle != null) //If there is a selected paddle
                        {
                            // Already in modifyMode
                            if (modifyMode)
                            {
                                switch (menuNum)
                                {
                                    case 0:
                                        unselectObject();
                                        modifyMode = false;
                                        break;
                                    case 1:
                                        if (rotateMode)
                                        {
                                            rotateMode = false;
                                            groundMarkerNode.RemoveChild(rotationTrans);
                                            menuSelected = false;
                                        }
                                        else
                                        {
                                            rotateMode = true;
                                            groundMarkerNode.AddChild(rotationTrans);
                                            menuSelected = true;
                                        }
                                        break;
                                    case 2:
                                        if (translateMode)
                                        {
                                            translateMode = false;
                                            groundMarkerNode.RemoveChild(rotationTrans);
                                            menuSelected = false;
                                        }
                                        else
                                        {
                                            translateMode = true;
                                            groundMarkerNode.AddChild(rotationTrans);
                                            menuSelected = true;
                                        }
                                        break;
                                    case 3:
                                        if (scaleMode)
                                        {
                                            scaleMode = false;
                                            menuSelected = false;
                                        }
                                        else
                                        {
                                            scaleMode = true;
                                            menuSelected = true;
                                        }
                                        break;
                                    case 4:
                                        if (noteMode)
                                        {
                                            noteMode = false;
                                            menuSelected = false;
                                        }
                                        else
                                        {
                                            noteMode = true;
                                            menuSelected = true;
                                        }
                                        break;
                                    case 5:
                                        deletePaddle();
                                        modifyMode = false;
                                        break;
                                    default:
                                        break;
                                }
                            }
                            else //Go to menu
                            {
                                modifyMode = true;
                            }
                        }
                            // Select an existing paddle and go into rough transform
                        else if (!systemMode && selectedPaddle == null && highlightedPaddle != null)
                        {
                            selectObject();
                        }
                            // Already in systemMode
                        else if (systemMode)
                        {
                            switch (sysMenuNum)
                            {
                                case 0:
                                    systemMode = false;
                                    break;

                                case 1:
                                    if (sysPathMode)
                                    {
                                        sysPathMode = false;
                                        sysMenuSelected = false;
                                        pathCount = 0;
                                        showPaths = false;
                                        foreach (Node n in balls)
                                        {
                                            TransformNode transNode = (TransformNode) n.Parent;
                                            if (transNode.Children.Count > 1)
                                                transNode.Children.RemoveRange(1, transNode.Children.Count - 1);
                                        }

                                    }
                                    else
                                    {
                                        sysPathMode = true;
                                        sysMenuSelected = true;
                                        pathCount = 0;
                                        showPaths = true;
                                        Vector2 pos = new Vector2(0, 0);
                                        Notifier.AddMessage("Showing Paths");
                                    }
                                    break;

                                case 2:
                                    saveConfiguration();
                                    Notifier.AddMessage("Configuration Saved");
                                    systemMode = false;
                                    break;

                                case 3:
                                    loadConfiguration();
                                    Notifier.AddMessage("Configuration Loaded");
                                    systemMode = false;
                                    break;

                                case 4:
                                    showHelp = true;
                                    systemMode = false;
                                    break;

                                default:
                                    break;
                            }
                        }
                        break;
                        //Middle click
                    case 1:
                        isGamePaused = !isGamePaused;
                        if (isGamePaused)
                        {
                            for (int i = 0; i < balls.Count; i++)
                            {
                                ((NewtonPhysics) scene.PhysicsEngine).DisableSimulation(balls[i].Physics);
                            }
                            pauseDropSpeed = ballDropSpeed;
                            ballDropSpeed = 0;
                        }
                        else
                        {
                            for (int i = 0; i < balls.Count; i++)
                            {
                                ((NewtonPhysics) scene.PhysicsEngine).EnableSimulation(balls[i].Physics);
                            }
                            ballDropSpeed = pauseDropSpeed;
                        }
                        break;

                        //Right click
                    case 2:
                        // trigger the modify menu of an existing paddle
                        if (highlightedDrop != null)
                        {
                            deleteDropPoint();
                        }
                        else if (selectedPaddle == null && highlightedPaddle != null && !systemMode)
                        {
                            selectObject();
                            modifyMode = true;
                        }
                            // close the modify menu
                        else if (selectedPaddle != null && modifyMode && !rotateMode && !translateMode && !scaleMode &&
                                 !noteMode && !systemMode)
                        {
                            unselectObject();
                            modifyMode = false;
                        }
                            // cancel the item in modifyMode
                        else if (selectedPaddle != null && modifyMode && !systemMode)
                        {
                            if (rotateMode)
                            {
                                rotateMode = false;
                                groundMarkerNode.RemoveChild(rotationTrans);
                                menuSelected = false;
                            }
                            if (scaleMode)
                            {
                                scaleMode = false;
                                menuSelected = false;
                            }
                            if (noteMode)
                            {
                                noteMode = false;
                                menuSelected = false;
                            }
                            if (translateMode)
                            {
                                translateMode = false;
                                groundMarkerNode.RemoveChild(rotationTrans);
                                menuSelected = false;
                            }
                        }
                            // turn on/off system menu
                        else if (selectedPaddle == null && !modifyMode)
                        {
                            if (highlightedPaddle == null && !systemMode)
                                systemMode = true;
                            else if (systemMode)
                                systemMode = false;
                        }
                            // delete the paddle
                        else if (selectedPaddle != null && !modifyMode)
                        {
                            deletePaddle();
                        }
                        break;

                    default:
                        break;
                }
            }
        }

        private void mouseWheelHandler(int x, int y) {
            x = x / 120;

            if (selectedPaddle != null && !modifyMode)
            {
                if (toolbarMarkerNode.MarkerFound)
                {
                    if (x == 1) scalePaddleUpModify();
                    else if (x == -1) scalePaddleDownModify();
                }
            }
            else if (selectedPaddle != null && modifyMode && !rotateMode && !translateMode && !scaleMode && !noteMode)
            {
                Sound.Play("button_2");
                if (x == -1)
                {
                    menuNum++;
                    if (menuNum > 5)
                        menuNum -= 6;
                }
                else if (x == 1)
                {
                    menuNum--;
                    if (menuNum < 0)
                        menuNum += 6;
                }
            }
            else if (selectedPaddle != null && modifyMode)
            {
                if (rotateMode)
                {
                    if (groundMarkerNode.MarkerFound)
                    {
                        Sound.Play("button_2");
                        if (x == 1) rotatePaddleUp();
                        else if (x == -1) rotatePaddleDown();
                    }
                }
                else if (translateMode)
                {
                    if (groundMarkerNode.MarkerFound)
                    {
                        Sound.Play("button_2");
                        if (x == 1) movePaddleFarther();
                        else if (x == -1) movePaddleCloser();
                    }
                }
                else if (scaleMode)
                {
                    if (groundMarkerNode.MarkerFound)
                    {
                        Sound.Play("button_2");
                        if (x == 1) scalePaddleUpModify();
                        else if (x == -1) scalePaddleDownModify();
                    }
                }
                else if (noteMode)
                {
                    int playNote = ((Paddle) selectedPaddle).Note;
                    if (groundMarkerNode.MarkerFound)
                    {
                        if (x == 1 && playNote < 27) playNote++;
                        else if (x == -1 && playNote > 0) playNote--;
                    }
                    if (playNote >= 27) playNote = 0;
                    if (x == 1)
                    {
                        playNote++;  //helpme
                        if (playNote > 26)
                            playNote -= 27;
                    }
                    else if (x == -1)
                    {
                        playNote--;
                        if (playNote < 0)
                            playNote += 27;
                    }
                    if (playNote <= 0)
                    {
                        selectedPaddle.Material.Diffuse = Color.Silver.ToVector4();
                    }
                    else
                    {
                        Vector3 trans = selectedPaddle.WorldTransformation.Translation;
                        ballPaddleCollision(playNote + offSet, trans);
                    }
                    selectedPaddle.Note = playNote;
                    ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(selectedPaddle.Physics);
                    ((NewtonPhysics)scene.PhysicsEngine).AddPhysicsObject(selectedPaddle.Physics);
                }
            }
            else if (systemMode)
            {
                Sound.Play("button_2");
                if (x == -1)
                {
                    sysMenuNum++;
                    if (sysMenuNum > 4)
                        sysMenuNum -= 5;
                }
                else if (x == 1)
                {
                    sysMenuNum--;
                    if (sysMenuNum < 0)
                        sysMenuNum += 5;
                }
            }
            else if (selectedPaddle == null)
            {
                ballDropSpeed = x > 0 ? ballDropSpeed + 1 : ballDropSpeed - 1;
                ballDropSpeed = ballDropSpeed < 0 ? 0 : ballDropSpeed;
                ballDropSpeed = ballDropSpeed > 9 ? 9 : ballDropSpeed;
                if (x > 0 && ballDropSpeed == 1) {
                    foreach (DropPoint d in dropPoints)
                    {
                        Ball b = d.createBall();
                        balls.Add(b);
                        groundMarkerNode.AddChild(b.Trans);
                        balls.Add(b);
                    }
                }
                Notifier.AddMessage("Changed ball drop speed to " + ballDropSpeed);
            }
        }

        private void mouseMoveHandler(Point target)
        {
            int quadrantSize = 5;
            if (target.X >= maxX - quadrantSize)

            {
                if (mouseGestureTime > 30)
                {
                    mapVisible = !mapVisible;
                    animateMapXIncrement = -80;
                    animateMapYIncrement = 0;
                    Sound.Play("button_1");
                    if (mapVisible) {
                        Notifier.AddMessage("MAP is ON. FLICK LOOP RIGHT to turn off.");
                    } else {
                        Notifier.AddMessage("MAP is OFF. FLICK LOOP RIGHT to turn on.");
                    }
                }
                mouseGestureTime = 0;
            } 
        }

        private void drawMap() {
            //Draw map in the right place
            if (mapVisible)
            {
                if ((mapX + animateMapXIncrement >= maxX - mapWidth && mapX + animateMapXIncrement <= maxX) || 
                    (mapX + animateMapXIncrement >= -mapWidth && mapX + animateMapXIncrement <= 0) )
                {
                    mapX += animateMapXIncrement;
                }
                spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                spriteBatch.Draw((Texture2D)zMapTexture, new Rectangle(mapX, zMapY, mapWidth, mapHeight), Color.White);
                spriteBatch.End();

                spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                spriteBatch.Draw((Texture2D)xMapTexture, new Rectangle(mapX, xMapY, mapWidth, mapHeight), Color.White);
                spriteBatch.End();

                spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                spriteBatch.Draw((Texture2D)yMapTexture, new Rectangle(mapX, yMapY, mapWidth, mapHeight), Color.White);
                spriteBatch.End();

            }
                
            else 
            {
                if ((mapX - animateMapXIncrement/2 >= maxX - mapWidth && mapX - animateMapXIncrement/2 <= maxX) ||
                (mapX - animateMapXIncrement >= -mapWidth && mapX - animateMapXIncrement <= 0))
                {
                    mapX -= (animateMapXIncrement / 2);
                    spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                    spriteBatch.Draw((Texture2D)zMapTexture, new Rectangle(mapX, zMapY, mapWidth, mapHeight), Color.White);
                    spriteBatch.End();

                    spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                    spriteBatch.Draw((Texture2D)xMapTexture, new Rectangle(mapX, xMapY, mapWidth, mapHeight), Color.White);
                    spriteBatch.End();

                    spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
                    spriteBatch.Draw((Texture2D)yMapTexture, new Rectangle(mapX, yMapY, mapWidth, mapHeight), Color.White);
                    spriteBatch.End();
                }
                else
                {
                    drawSneakMaps();
                }

            }
            
        }

        private void drawSneakMaps()
        {
            int sneakPreviewWidth = 10;
            spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
            spriteBatch.Draw((Texture2D)zMapTexture, new Rectangle(maxX - sneakPreviewWidth, zMapY, mapWidth, mapHeight), Color.White);
            spriteBatch.End();

            spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
            spriteBatch.Draw((Texture2D)xMapTexture, new Rectangle(maxX - sneakPreviewWidth, xMapY, mapWidth, mapHeight), Color.White);
            spriteBatch.End();

            spriteBatch.Begin(SpriteBlendMode.None, SpriteSortMode.Immediate, SaveStateMode.None);
            spriteBatch.Draw((Texture2D)yMapTexture, new Rectangle(maxX - sneakPreviewWidth, yMapY, mapWidth, mapHeight), Color.White);
            spriteBatch.End();

        }

        private void selectObject()
        {
            if (highlightedPaddle == null) return;
            //Vector3 paddlescale, objectTranslate;
            //Quaternion objectRotate;

            GeometryNode o = highlightedPaddle;
            TransformNode t = (TransformNode)o.Parent;
            MarkerNode marker = (MarkerNode)t.Parent;

            /* Get the object's World location w.r.t camera */
            Matrix m = t.WorldTransformation * marker.WorldTransformation;
            //m.Decompose(out paddlescale, out objectRotate, out objectTranslate);

            //Vector3 objOld = t.Translation;
            Vector3 objWorld = m.Translation;
            Vector3 markerWorld = toolbarMarkerNode.WorldTransformation.Translation;


            /* To check if the object's world location is still the same */
            m = t.WorldTransformation * toolbarMarkerNode.WorldTransformation;
            markerWorld = toolbarMarkerNode.WorldTransformation.Translation;

            /* Change the state of object */
            selectedPaddle = highlightedPaddle;
            selectedNodeColor = highlightedNodeColor;
            selectedPaddle.Material.Diffuse = Color.RoyalBlue.ToVector4();
            highlightedPaddle = null;
            idleCount = 0;
            isPaddleSelected = true;
        }

        private void unselectObject()
        {
            if (isPaddleSelected && highlightedPaddle == null && selectedPaddle != null)
            {
                if (!groundMarkerNode.MarkerFound || (!modifyMode && !toolbarMarkerNode.MarkerFound) || selectedPaddle == null) return;
                Paddle o = selectedPaddle;
                TransformNode t = (TransformNode)o.Parent;
                MarkerNode marker = (MarkerNode)t.Parent;

                /* Get the object's World location w.r.t camera */
                Matrix m = t.WorldTransformation * marker.WorldTransformation;

                Vector3 objWorld = m.Translation;
                Vector3 markerWorld = toolbarMarkerNode.WorldTransformation.Translation;

                if (!modifyMode)
                {
                    Matrix scaleFactor = Matrix.CreateScale(selectedPaddle.Trans.Scale);
                    Matrix mat = scaleFactor * toolbarMarkerNode.WorldTransformation*
                                 Matrix.Invert(groundMarkerNode.WorldTransformation);
                    Vector3 sca, trans;
                    Quaternion rot;
                    mat.Decompose(out sca, out rot, out trans);
                    t.Translation = trans;
                    t.Rotation = rot;
                    t.Scale = sca;
                }

                /* Seamlesly Move the object into the Toolbar's world - i.e The world location before and after the transfer should be the same */
              //t.WorldTransformation = m * Matrix.Invert(groundMarkerNode.WorldTransformation);
                

                /* Move them in the scene graph too */
              //toolbarMarkerNode.RemoveChild(t);
              //groundMarkerNode.AddChild(t);

                /* To check if the object's world location is still the same */
                m = t.WorldTransformation * groundMarkerNode.WorldTransformation;
                markerWorld = toolbarMarkerNode.WorldTransformation.Translation;

                /* Change the state of object */
                if (selectedPaddle.Note != 0)
                {
                    selectedPaddle.Material.Diffuse = new Vector4(1f, (selectedPaddle.Note) / 25f, 0, 1f);
                }
                else 
                {
                    selectedPaddle.Material.Diffuse = selectedNodeColor;  //findme
                }

                isPaddleSelected = false;
                selectedPaddle = null;
                highlightedPaddle = null;
                modifyMode = false;
                //Vector3 nearSource = new Vector3(mouseLocation.X, mouseLocation.Y, 0);
            }
        }

        protected void saveConfiguration()
        {
            XmlTextWriter writer = new XmlTextWriter("paddleConfig.xml", null);
            writer.Formatting = Formatting.Indented;

            //Save paddles
            writer.WriteStartDocument();
            writer.WriteStartElement("config");
            foreach(Paddle p in paddles){
                TransformNode padTrans = (TransformNode)p.Parent;
                writer.WriteStartElement("paddle");
                writer.WriteAttributeString("note", p.Note.ToString());
                writer.WriteStartElement("translation");
                writer.WriteAttributeString("X", padTrans.Translation.X.ToString());
                writer.WriteAttributeString("Y", padTrans.Translation.Y.ToString());
                writer.WriteAttributeString("Z", padTrans.Translation.Z.ToString());
                writer.WriteEndElement();
                writer.WriteStartElement("rotation");
                writer.WriteAttributeString("X", padTrans.Rotation.X.ToString());
                writer.WriteAttributeString("Y", padTrans.Rotation.Y.ToString());
                writer.WriteAttributeString("Z", padTrans.Rotation.Z.ToString());
                writer.WriteAttributeString("W", padTrans.Rotation.W.ToString());
                writer.WriteEndElement();
                writer.WriteStartElement("scale");
                writer.WriteAttributeString("X", padTrans.Scale.X.ToString());
                writer.WriteAttributeString("Y", padTrans.Scale.Y.ToString());
                writer.WriteAttributeString("Z", padTrans.Scale.Z.ToString());
                writer.WriteEndElement();
                writer.WriteEndElement();
            }
            foreach(DropPoint d in dropPoints)
            {
                TransformNode dTrans = d.Trans;
                writer.WriteStartElement("drop");
                writer.WriteStartElement("translation");
                writer.WriteAttributeString("X", dTrans.Translation.X.ToString());
                writer.WriteAttributeString("Y", dTrans.Translation.Y.ToString());
                writer.WriteAttributeString("Z", dTrans.Translation.Z.ToString());
                writer.WriteEndElement();
                writer.WriteEndElement();
            }
            writer.WriteEndElement();
            writer.WriteEndDocument();
            writer.Close();
        }

        protected void loadConfiguration()
        {
            //First clear all the paddles
            foreach (Paddle p in paddles)
            {
                p.AddToPhysicsEngine = false;
                paddleTransforms.Remove((TransformNode)p.Parent);
                groundMarkerNode.RemoveChild(p.Parent);
                ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(p.Physics);
            }
            paddles.Clear();
            foreach (DropPoint d in dropPoints)
            {
                d.AddToPhysicsEngine = false;
                groundMarkerNode.RemoveChild(d.Trans);
                ((NewtonPhysics)scene.PhysicsEngine).RemovePhysicsObject(d.Physics);
            }
            dropPoints.Clear();

            //And now read paddles
            TransformNode t = new TransformNode();
            float x, y, z, w;
            int note = 0;
            string noteString;
            XmlTextReader reader = new XmlTextReader("paddleConfig.xml");
            reader.ReadStartElement();
            while (reader.Read())
            {
                if (reader.NodeType == XmlNodeType.Element)
                {
                    switch(reader.LocalName)
                    {
                        case "paddle":
                            t = new TransformNode();
                            noteString = reader.GetAttribute("note");
                            if (noteString != null)
                            {
                                note = int.Parse(noteString);
                            }
                            break;
                        case "drop":
                            t = new TransformNode();
                            break;
                        case "translation":
                            x = float.Parse(reader.GetAttribute("X"));
                            y = float.Parse(reader.GetAttribute("Y"));
                            z = float.Parse(reader.GetAttribute("Z"));
                            t.Translation = new Vector3(x, y, z);
                            break;
                        case "rotation":
                            x = float.Parse(reader.GetAttribute("X"));
                            y = float.Parse(reader.GetAttribute("Y"));
                            z = float.Parse(reader.GetAttribute("Z"));
                            w = float.Parse(reader.GetAttribute("W"));
                            t.Rotation = new Quaternion(x, y, z, w);
                            break;
                        case "scale":
                            x = float.Parse(reader.GetAttribute("X"));
                            y = float.Parse(reader.GetAttribute("Y"));
                            z = float.Parse(reader.GetAttribute("Z"));
                            t.Scale = new Vector3(x, y, z);
                            break;
                    }
                }
                else if (reader.NodeType == XmlNodeType.EndElement)
                {
                    if (reader.LocalName == "paddle")
                        createPaddle(t, note);
                    else if (reader.LocalName == "drop")
                        createSource(t.Translation);
                }
            }

            reader.Close();
        }

        /*private PrimitiveMesh createBox(float xdim, float ydim, float zdim)
        {
            PrimitiveMesh mesh = new PrimitiveMesh();

            VertexPositionNormal[] vertices = new VertexPositionNormal[24];
            Vector3 halfExtent = new Vector3();

            halfExtent.X = xdim / 2;
            halfExtent.Y = ydim / 2;
            halfExtent.Z = zdim / 2;

            Vector3 v0 = Vector3Helper.Get(-halfExtent.X, -halfExtent.Y, -halfExtent.Z);
            Vector3 v1 = Vector3Helper.Get(halfExtent.X, -halfExtent.Y, -halfExtent.Z);
            Vector3 v2 = Vector3Helper.Get(-halfExtent.X, halfExtent.Y, -halfExtent.Z);
            Vector3 v3 = Vector3Helper.Get(halfExtent.X, halfExtent.Y, -halfExtent.Z);
            Vector3 v4 = Vector3Helper.Get(halfExtent.X, halfExtent.Y, halfExtent.Z);
            Vector3 v5 = Vector3Helper.Get(-halfExtent.X, halfExtent.Y, halfExtent.Z);
            Vector3 v6 = Vector3Helper.Get(halfExtent.X, -halfExtent.Y, halfExtent.Z);
            Vector3 v7 = Vector3Helper.Get(-halfExtent.X, -halfExtent.Y, halfExtent.Z);

            Vector3 nZ = -Vector3.UnitZ;
            Vector3 pZ = Vector3.UnitZ;
            Vector3 nX = -Vector3.UnitX;
            Vector3 pX = Vector3.UnitX;
            Vector3 nY = -Vector3.UnitY;
            Vector3 pY = Vector3.UnitY;

            vertices[0].Position = v0; vertices[1].Position = v1;
            vertices[2].Position = v2; vertices[3].Position = v3;

            vertices[4].Position = v0; vertices[5].Position = v7;
            vertices[6].Position = v2; vertices[7].Position = v5;

            vertices[8].Position = v4; vertices[9].Position = v5;
            vertices[10].Position = v7; vertices[11].Position = v6;

            vertices[12].Position = v4; vertices[13].Position = v3;
            vertices[14].Position = v1; vertices[15].Position = v6;

            vertices[16].Position = v2; vertices[17].Position = v4;
            vertices[18].Position = v5; vertices[19].Position = v3;

            vertices[20].Position = v0; vertices[21].Position = v1;
            vertices[22].Position = v6; vertices[23].Position = v7;

            for (int i = 0; i < 4; i++)
                vertices[i].Normal = nZ;
            for (int i = 4; i < 8; i++)
                vertices[i].Normal = nX;
            for (int i = 8; i < 12; i++)
                vertices[i].Normal = pZ;
            for (int i = 12; i < 16; i++)
                vertices[i].Normal = pX;
            for (int i = 16; i < 20; i++)
                vertices[i].Normal = pY;
            for (int i = 20; i < 24; i++)
                vertices[i].Normal = nY;

            mesh.VertexDeclaration = new VertexDeclaration(State.Device,
                VertexPositionNormal.VertexElements);

            mesh.VertexBuffer = new VertexBuffer(State.Device,
                VertexPositionNormal.SizeInBytes * 24, BufferUsage.None);
            mesh.VertexBuffer.SetData(vertices);

            short[] indices = new short[36];

            indices[0] = 0; indices[1] = 1; indices[2] = 2;
            indices[3] = 2; indices[4] = 1; indices[5] = 3;
            indices[6] = 4; indices[7] = 6; indices[8] = 5;
            indices[9] = 6; indices[10] = 7; indices[11] = 5;
            indices[12] = 11; indices[13] = 10; indices[14] = 9;
            indices[15] = 11; indices[16] = 9; indices[17] = 8;
            indices[18] = 14; indices[19] = 15; indices[20] = 13;
            indices[21] = 15; indices[22] = 12; indices[23] = 13;
            indices[24] = 19; indices[25] = 17; indices[26] = 18;
            indices[27] = 19; indices[28] = 18; indices[29] = 16;
            indices[30] = 21; indices[31] = 20; indices[32] = 23;
            indices[33] = 21; indices[34] = 23; indices[35] = 22;

            mesh.IndexBuffer = new IndexBuffer(State.Device, typeof(short), 36,
                BufferUsage.None);
            mesh.IndexBuffer.SetData(indices);

            mesh.SizeInBytes = VertexPositionNormal.SizeInBytes;
            mesh.NumberOfVertices = 24;
            mesh.NumberOfPrimitives = 12;

            return mesh;
        }*/

        private void writeStatusMessageTop(String text) {
            UI2DRenderer.WriteText(Vector2.Zero, text, messageColor, font, GoblinEnums.HorizontalAlignment.Center, GoblinEnums.VerticalAlignment.Top);
        }

        private void writeStatusMessageBottom(String text) {
            UI2DRenderer.WriteText(Vector2.Zero, text, messageColor, font, GoblinEnums.HorizontalAlignment.Center, GoblinEnums.VerticalAlignment.Bottom);
        }

        private void writeStatusMessageTopLeft(String text)
        {
            UI2DRenderer.WriteText(Vector2.Zero, text, messageColor, font, GoblinEnums.HorizontalAlignment.Left, GoblinEnums.VerticalAlignment.Top);
        }

    }

}
